#include "stm32f10x.h" 
#include "Motor.h"

#define     Kp1 0.1
#define     Ki1 0.001
#define     Kd1 0.2

#define     Kpv1 1
#define     Kiv1 0.01
#define     Kdv1 0.2

//--------------------------

#define     Kp2 0.1
#define     Ki2 0.001
#define     Kd2 0.2

#define     Kpv2 1
#define     Kiv2 0.01
#define     Kdv2 0.2

extern int16_t Real_Speed1,Real_Position1,Target_Position1;
extern int16_t Real_Speed2,Real_Position2,Target_Position2;

int16_t Control1(void)
{
    static int16_t moto;

    Real_Speed1=Encoder1_Get();
    Real_Position1 += Real_Speed1;

    moto=Pid1(Real_Position1,Target_Position1);

    if(Real_Position1-Target_Position1==0)//到位
    {
        moto=0;
    }
    else
    {
        SpeedPid1(Real_Speed1,moto);    
    
        //-------------限幅--------------
        if(moto>5000)
        {
            moto=5000;
        }
        else if(moto<-5000)
        {
            moto=-5000;
        }
        Motor1_SetSpeed(moto);
        return Real_Position1;
    }
}


int16_t Pid1 (int16_t Real_P,int16_t Target_P)
{
    static float erro,erro_last,I_erro=0,moto;

    erro=Target_P-Real_P;
    I_erro+=erro;//偏差累计

    if(I_erro>5000)//限幅
    {
        I_erro=5000;
    }
    else if(I_erro<-5000)
    {
        I_erro=-5000;
    }

    moto=Kp1*erro+Ki1*I_erro+Kd1*(erro-erro_last);

    erro_last=erro;
    return moto;
}   

int16_t SpeedPid1 (int16_t Real_V,int16_t Target_V)
{
    static float erro,erro_last=0,erro_last_last=0,pwm;

    erro=Target_V-Real_V;


    pwm=Kpv1*(erro-erro_last)+Kiv1*erro+Kdv1*(erro-2*erro_last+erro_last_last);

    erro_last_last=erro_last;
    erro_last=erro;
    return pwm;
} 




int16_t Control2(void)
{
    static int16_t moto;

    Real_Speed2=Encoder2_Get();
    Real_Position2 += Real_Speed2;

    moto=Pid2(Real_Position2,Target_Position2);

    if(Real_Position2-Target_Position2==0)//到位
    {
        moto=0;
    }
    else
    {
        SpeedPid2(Real_Speed2,moto);    
    
        //-------------限幅--------------
        if(moto>5000)
        {
            moto=5000;
        }
        else if(moto<-5000)
        {
            moto=-5000;
        }
        Motor2_SetSpeed(moto);
        return Real_Position2;
    }
}

int16_t Pid2 (int16_t Real_P,int16_t Target_P)
{
    static float erro,erro_last,I_erro=0,moto;

    erro=Target_P-Real_P;
    I_erro+=erro;//偏差累计

    if(I_erro>5000)//限幅
    {
        I_erro=5000;
    }
    else if(I_erro<-5000)
    {
        I_erro=-5000;
    }

    moto=Kp2*erro+Ki2*I_erro+Kd2*(erro-erro_last);

    erro_last=erro;
    return moto;
}   

int16_t SpeedPid2 (int16_t Real_V,int16_t Target_V)
{
    static float erro,erro_last=0,erro_last_last=0,pwm;

    erro=Target_V-Real_V;


    pwm=Kpv2*(erro-erro_last)+Kiv2*erro+Kdv2*(erro-2*erro_last+erro_last_last);

    erro_last_last=erro_last;
    erro_last=erro;
    return pwm;
} 
